% Calculate the configuration variables of 2D knee point feet model
% StanceLegIndex: 1--right leg is the stance leg
%                 2--left leg is the stance leg
function ret = Knee2D_angle(xpos,ypos,zpos,sns_brk,StanceLegIndex)

StartPt = sns_brk(2);
EndPt = sns_brk(3);

for i = 1:length(xpos.lknee)
    % right leg is the stance leg
    if StanceLegIndex == 1
        % stance angle
        sa_angle(i) = StanceAnkleAngle(xpos.rknee(i),ypos.rknee(i),xpos.rankle_avg(i),ypos.rankle_avg(i));
% %         if i==263
% %          xpos.rknee(263)
% %          ypos.rknee(263)
% %          xpos.rankle_avg(263)
% %          ypos.rankle_avg(263)
% %         end
        % stance knee
        sknee_angle(i) = StanceKneeAngle(xpos.rknee(i),ypos.rknee(i),xpos.rhip(i),ypos.rhip(i),...
            xpos.rankle_avg(i),ypos.rankle_avg(i));
        
        % hip
%         hip_angle(i) = HipAngle(xpos.rknee(i),ypos.rknee(i),xpos.hip_avg(i),ypos.hip_avg(i),...
%             xpos.lknee(i),ypos.lknee(i));
        
        % non-stance knee
        nsknee_angle(i) = NonStanceKneeAngle(xpos.lknee(i),ypos.lknee(i),xpos.lhip(i),ypos.lhip(i),...
            xpos.lankle_avg(i),ypos.lankle_avg(i));
        
        % torso angle theta 3
        theta3(i) = TorsoAngle3(xpos.rknee(i),ypos.rknee(i),xpos.rhip(i),ypos.rhip(i),...
            xpos.hip_avg(i),ypos.hip_avg(i),xpos.torso(i),ypos.torso(i));
        % torso angle theta 4
        theta4(i) = TorsoAngle4(xpos.lknee(i),ypos.lknee(i),xpos.lhip(i),ypos.lhip(i),...
            xpos.hip_avg(i),ypos.hip_avg(i),xpos.torso(i),ypos.torso(i));
        
        
    else % left leg is the stance leg
        
        
        % stance angle
        sa_angle(i) = StanceAnkleAngle(xpos.lknee(i),ypos.lknee(i),xpos.lankle_avg(i),ypos.lankle_avg(i));
        
        % stance knee
        sknee_angle(i) = StanceKneeAngle(xpos.lknee(i),ypos.lknee(i),xpos.lhip(i),ypos.lhip(i),...
            xpos.lankle_avg(i),ypos.lankle_avg(i));
        % hip
%         hip_angle(i) = HipAngle(xpos.lknee(i),ypos.lknee(i),xpos.hip_avg(i),ypos.hip_avg(i),...
%             xpos.rknee(i),ypos.rknee(i));
        
        
        % non-stance knee
        nsknee_angle(i) = NonStanceKneeAngle(xpos.rknee(i),ypos.rknee(i),xpos.rhip(i),ypos.rhip(i),...
            xpos.rankle_avg(i),ypos.rankle_avg(i));
        
        % torso angle theta 3
        theta3(i) = TorsoAngle3(xpos.lknee(i),ypos.lknee(i),xpos.lhip(i),ypos.lhip(i),...
            xpos.hip_avg(i),ypos.hip_avg(i),xpos.torso(i),ypos.torso(i));
        
        % torso angle theta 4
        theta4(i) = TorsoAngle4(xpos.rknee(i),ypos.rknee(i),xpos.rhip(i),ypos.rhip(i),...
            xpos.hip_avg(i),ypos.hip_avg(i),xpos.torso(i),ypos.torso(i));
        
        %
    end
    % torso hip angle
        TorsoHip(i) = TorsoHipAngle(xpos.torso(i),ypos.torso(i),xpos.hip_avg(i),ypos.hip_avg(i));
        
    
end

hip_angle = theta3 - theta4;
ret.sa = sa_angle(StartPt:EndPt);
ret.sknee = sknee_angle(StartPt:EndPt);
ret.nsknee = nsknee_angle(StartPt:EndPt);
ret.hip = hip_angle(StartPt:EndPt);
ret.theta3 = theta3(StartPt:EndPt);
ret.theta4 = theta4(StartPt:EndPt);
ret.torso_hip = TorsoHip(StartPt:EndPt);
end


% calculate the angle of knee, use avererage position of ankle
function ret = StanceKneeAngle(xknee,yknee,xhip,yhip,xankle,yankle)
        pt1 = [xknee;yknee;0];
        pt2 = [xhip;yhip;0];
        pt3 = [xankle;yankle;0];
        [knee_ang,len1,len2] = relAngle(pt2,pt1,pt3);
        ret = knee_ang;
end

% calculate the angle of the stance ankle, use avererage position of ankle
function ret = StanceAnkleAngle(xknee,yknee,xankle,yankle)
   ret = -atan2((xknee-xankle),(yknee-yankle));
% ret = -atan((xknee-xankle),(yknee-yankle));

end

function ret = NonStanceKneeAngle (xknee,yknee,xhip,yhip,xankle,yankle)
    ret = StanceKneeAngle (xknee,yknee,xhip,yhip,xankle,yankle);
end


% calculate the hip angle, use the average hip position
function ret = HipAngle(xsknee,ysknee,xhip,yhip,xnsknee,ynsknee)
        pt1 = [xsknee; ysknee ;0];
        pt2 = [xhip;yhip;0];
        pt3 = [xnsknee;ynsknee;0];
        [hip_ang,len1,len2] = relAngle(pt1,pt2,pt3);
        ret = hip_ang;
end

% torso related angle
% calculate the angle of theta3
function ret = TorsoAngle3(xknee,yknee,xhip,yhip,xhip_avg,yhip_avg,xtorso,ytorso)
    pt1 = [xknee;yknee;0]; % stance knee
    pt2 = [xhip;yhip;0];
    pt3 = [xhip_avg;yhip_avg;0];
    pt4 = [xtorso;ytorso;0]; % torso
    [theta_3,len1,len2] = relAngle2(pt1,pt2,pt3,pt4);
    ret = -theta_3;
end

% torso related angle
% calculate the angle of theta4
function ret = TorsoAngle4(xknee,yknee,xhip,yhip,xhip_avg,yhip_avg,xtorso,ytorso)
    ret = TorsoAngle3(xknee,yknee,xhip,yhip,xhip_avg,yhip_avg,xtorso,ytorso);
end

% calculate the angle of the torso to the hip average
function ret = TorsoHipAngle(xtorso,ytorso,xhip,yhip)
    ret = -atan2((xtorso-xhip),(ytorso-yhip));
end